/*
 * Robot.cpp
 *
 *  Created on: Mar 14, 2010
 *      Author: As_You_Wish
 */

#include "Robot.h"
extern "C" {
#include "SoR_Utils.h"
#include "timer640.h"
#include "rprintf.h"
#include <avr/io.h>
#include "Time.h"
}
#include "global_defs.h"
#include "Sensor1bit.h"
#include "Sensor10bit.h"

Robot::Robot(Brain* brain) {
	initServos();
	initSensors();
	initStopAction();
	initWalkActions();
	initStandUpActions();
	this->brain = brain;
	this->brain->robot = this;
	setAction(stop_action);
}

Robot::~Robot() {
	for (int i = 0; i < NUM_SERVOS; i++) {
		delete (stop_action[i]);
		delete (walk_action[i]);
		delete (stand_up_action[i]);
		delete (left_stand_up_action[i]);
		delete (right_stand_up_action[i]);
		delete (servos[i]);
	}
	delete (stop_action);
	delete (walk_action);
	delete (stand_up_action);
	delete (left_stand_up_action);
	delete (right_stand_up_action);
	delete (servos);
	delete (button);
}

void Robot::initServos() {
	servos = new Servo*[NUM_SERVOS];
	servos[SERVO_UFRL] = new Servo((char*) "SERVO_UFRL", 590, &PORTA, 4);
	servos[SERVO_LFRL] = new Servo((char*) "SERVO_LFRL", -420, &PORTA, 7);
	servos[SERVO_UBRL] = new Servo((char*) "SERVO_UBRL", 700, &PORTC, 6);
	servos[SERVO_LBRL] = new Servo((char*) "SERVO_LBRL", -430, &PORTC, 3);
	servos[SERVO_UFLL] = new Servo((char*) "SERVO_UFLL", -765, &PORTE, 3);
	servos[SERVO_LFLL] = new Servo((char*) "SERVO_LFLL", 875, &PORTE, 6);
	servos[SERVO_UBLL] = new Servo((char*) "SERVO_UBLL", -700, &PORTH, 3);
	servos[SERVO_LBLL] = new Servo((char*) "SERVO_LBLL", 950, &PORTH, 6);
	servos[SERVO_UN] = new Servo((char*) "SERVO_UN", 800, &PORTA, 3);
	servos[SERVO_LN] = new Servo((char*) "SERVO_LN", 670, &PORTE, 2);
	servos[SERVO_T] = new Servo((char*) "SERVO_T", 700, &PORTC, 1);

}

void Robot::initStopAction() {
	stop_action = new ServoAction*[NUM_SERVOS];
	for (int i = 0; i < NUM_SERVOS; i++) {
		stop_action[i] = new ServoAction(servos[i]);
	}
	stop_action[SERVO_UFLL]->add(true, 0, 500);
	stop_action[SERVO_UFRL]->add(true, 0, 500);
	stop_action[SERVO_LFLL]->add(true, 0, 500);
	stop_action[SERVO_LFRL]->add(true, 0, 500);
	stop_action[SERVO_UBLL]->add(true, 0, 500);
	stop_action[SERVO_UBRL]->add(true, 0, 500);
	stop_action[SERVO_LBLL]->add(true, 0, 500);
	stop_action[SERVO_LBRL]->add(true, 0, 500);
	stop_action[SERVO_LN]->add(true, 0, 500);
	stop_action[SERVO_UN]->add(true, 0, 500);
	stop_action[SERVO_T]->add(true, 0, 500);
}

void Robot::initWalkActions() {
	walk_action = new ServoAction*[NUM_SERVOS];
	turn_walk_action = new ServoAction*[NUM_SERVOS];
	for (int i = 0; i < NUM_SERVOS; i++) {
		walk_action[i] = new ServoAction(servos[i]);
		turn_walk_action[i] = new ServoAction(servos[i]);
	}

	// Walk action
	walk_pos0(walk_action, SERVO_UFLL, SERVO_LFLL, 250);
	walk_pos2(walk_action, SERVO_UFRL, SERVO_LFRL, 250);
	walk_pos0(walk_action, SERVO_UBRL, SERVO_LBRL, 250);
	walk_pos2(walk_action, SERVO_UBLL, SERVO_LBLL, 250);

	walk_pos1(walk_action, SERVO_UFLL, SERVO_LFLL, 250);
	walk_pos3(walk_action, SERVO_UFRL, SERVO_LFRL, 250);
	walk_pos1(walk_action, SERVO_UBRL, SERVO_LBRL, 250);
	walk_pos3(walk_action, SERVO_UBLL, SERVO_LBLL, 250);

	walk_pos2(walk_action, SERVO_UFLL, SERVO_LFLL, 250);
	walk_pos0(walk_action, SERVO_UFRL, SERVO_LFRL, 250);
	walk_pos2(walk_action, SERVO_UBRL, SERVO_LBRL, 250);
	walk_pos0(walk_action, SERVO_UBLL, SERVO_LBLL, 250);

	walk_pos3(walk_action, SERVO_UFLL, SERVO_LFLL, 250);
	walk_pos1(walk_action, SERVO_UFRL, SERVO_LFRL, 250);
	walk_pos3(walk_action, SERVO_UBRL, SERVO_LBRL, 250);
	walk_pos1(walk_action, SERVO_UBLL, SERVO_LBLL, 250);


	// Turn left/right walk action
	walk_pos3(turn_walk_action, SERVO_UFLL, SERVO_LFLL, 250);
	walk_pos1(turn_walk_action, SERVO_UBLL, SERVO_LBLL, 250);
	walk_pos2(turn_walk_action, SERVO_UFRL, SERVO_LFRL, 250);
	walk_pos0(turn_walk_action, SERVO_UBRL, SERVO_LBRL, 250);

	walk_pos2(turn_walk_action, SERVO_UFLL, SERVO_LFLL, 250);
	walk_pos0(turn_walk_action, SERVO_UBLL, SERVO_LBLL, 250);
	walk_pos3(turn_walk_action, SERVO_UFRL, SERVO_LFRL, 250);
	walk_pos1(turn_walk_action, SERVO_UBRL, SERVO_LBRL, 250);

	walk_pos1(turn_walk_action, SERVO_UFLL, SERVO_LFLL, 250);
	walk_pos3(turn_walk_action, SERVO_UBLL, SERVO_LBLL, 250);
	walk_pos0(turn_walk_action, SERVO_UFRL, SERVO_LFRL, 250);
	walk_pos2(turn_walk_action, SERVO_UBRL, SERVO_LBRL, 250);

	walk_pos0(turn_walk_action, SERVO_UFLL, SERVO_LFLL, 250);
	walk_pos2(turn_walk_action, SERVO_UBLL, SERVO_LBLL, 250);
	walk_pos1(turn_walk_action, SERVO_UFRL, SERVO_LFRL, 250);
	walk_pos3(turn_walk_action, SERVO_UBRL, SERVO_LBRL, 250);




	walk_action[SERVO_LN]->add(true, 0, 1000);
	walk_action[SERVO_UN]->add(true, 0, 1000);

	walk_action[SERVO_T]->add(true, 0, 1000);

}

void Robot::initStandUpActions() {
	left_stand_up_action = new ServoAction*[NUM_SERVOS];
	for (int i = 0; i < NUM_SERVOS; i++) {
		left_stand_up_action[i] = new ServoAction(servos[i]);
	}

	// Step 1: Tilt to the right
	left_stand_up_action[SERVO_UFLL]->add(true, 450, 1000);
	left_stand_up_action[SERVO_UFRL]->add(true, 0, 1000);
	left_stand_up_action[SERVO_LFLL]->add(true, 0, 1000);
	left_stand_up_action[SERVO_LFRL]->add(true, 0, 1000);
	left_stand_up_action[SERVO_UBLL]->add(true, -450, 1000);
	left_stand_up_action[SERVO_UBRL]->add(true, 0, 1000);
	left_stand_up_action[SERVO_LBLL]->add(true, 0, 1000);
	left_stand_up_action[SERVO_LBRL]->add(true, 0, 1000);
	left_stand_up_action[SERVO_LN]->add(true, 0, 1000);
	left_stand_up_action[SERVO_UN]->add(true, 0, 1000);
	left_stand_up_action[SERVO_T]->add(true, 500, 1000);

	// Step 2: legs straight forward and back
	left_stand_up_action[SERVO_UFLL]->add(true, 350, 1000);
	left_stand_up_action[SERVO_UFRL]->add(true, 350, 1000);
	left_stand_up_action[SERVO_LFLL]->add(true, 0, 1000);
	left_stand_up_action[SERVO_LFRL]->add(true, 0, 1000);
	left_stand_up_action[SERVO_UBLL]->add(true, -350, 1000);
	left_stand_up_action[SERVO_UBRL]->add(true, -350, 1000);
	left_stand_up_action[SERVO_LBLL]->add(true, 0, 1000);
	left_stand_up_action[SERVO_LBRL]->add(true, 0, 1000);
	left_stand_up_action[SERVO_LN]->add(true, 0, 1000);
	left_stand_up_action[SERVO_UN]->add(true, 0, 1000);
	left_stand_up_action[SERVO_T]->add(true, -450, 1000);

	// Step 3: Stand up
//	left_stand_up_action[SERVO_UFLL]->add(true, 0, 1000);
//	left_stand_up_action[SERVO_UFRL]->add(true, 0, 1000);
//	left_stand_up_action[SERVO_LFLL]->add(true, 0, 1000);
//	left_stand_up_action[SERVO_LFRL]->add(true, 0, 1000);
//	left_stand_up_action[SERVO_UBLL]->add(true, 0, 1000);
//	left_stand_up_action[SERVO_UBRL]->add(true, 0, 1000);
//	left_stand_up_action[SERVO_LBLL]->add(true, 0, 1000);
//	left_stand_up_action[SERVO_LBRL]->add(true, 0, 1000);
//	left_stand_up_action[SERVO_LN]->add(true, 0, 1000);
//	left_stand_up_action[SERVO_UN]->add(true, 0, 1000);
//	left_stand_up_action[SERVO_T]->add(true, 0, 1000);


	right_stand_up_action = new ServoAction*[NUM_SERVOS];
	for (int i = 0; i < NUM_SERVOS; i++) {
		right_stand_up_action[i] = new ServoAction(servos[i]);
	}

	// Step 1: Tilt to the left
	right_stand_up_action[SERVO_UFLL]->add(true, 0, 1000);
	right_stand_up_action[SERVO_UFRL]->add(true, 450, 1000);
	right_stand_up_action[SERVO_LFLL]->add(true, 0, 1000);
	right_stand_up_action[SERVO_LFRL]->add(true, 0, 1000);
	right_stand_up_action[SERVO_UBLL]->add(true, 0, 1000);
	right_stand_up_action[SERVO_UBRL]->add(true, -450, 1000);
	right_stand_up_action[SERVO_LBLL]->add(true, 0, 1000);
	right_stand_up_action[SERVO_LBRL]->add(true, 0, 1000);
	right_stand_up_action[SERVO_LN]->add(true, 0, 1000);
	right_stand_up_action[SERVO_UN]->add(true, 0, 1000);
	right_stand_up_action[SERVO_T]->add(true, -500, 1000);

	// Step 2: legs straight forward and back
	right_stand_up_action[SERVO_UFLL]->add(true, 350, 1000);
	right_stand_up_action[SERVO_UFRL]->add(true, 350, 1000);
	right_stand_up_action[SERVO_LFLL]->add(true, 0, 1000);
	right_stand_up_action[SERVO_LFRL]->add(true, 0, 1000);
	right_stand_up_action[SERVO_UBLL]->add(true, -350, 1000);
	right_stand_up_action[SERVO_UBRL]->add(true, -350, 1000);
	right_stand_up_action[SERVO_LBLL]->add(true, 0, 1000);
	right_stand_up_action[SERVO_LBRL]->add(true, 0, 1000);
	right_stand_up_action[SERVO_LN]->add(true, 0, 1000);
	right_stand_up_action[SERVO_UN]->add(true, 0, 1000);
	right_stand_up_action[SERVO_T]->add(true, 450, 1000);

	// Step 3: Stand up
//	right_stand_up_action[SERVO_UFLL]->add(true, 0, 1000);
//	right_stand_up_action[SERVO_UFRL]->add(true, 0, 1000);
//	right_stand_up_action[SERVO_LFLL]->add(true, 0, 1000);
//	right_stand_up_action[SERVO_LFRL]->add(true, 0, 1000);
//	right_stand_up_action[SERVO_UBLL]->add(true, 0, 1000);
//	right_stand_up_action[SERVO_UBRL]->add(true, 0, 1000);
//	right_stand_up_action[SERVO_LBLL]->add(true, 0, 1000);
//	right_stand_up_action[SERVO_LBRL]->add(true, 0, 1000);
//	right_stand_up_action[SERVO_LN]->add(true, 0, 1000);
//	right_stand_up_action[SERVO_UN]->add(true, 0, 1000);
//	right_stand_up_action[SERVO_T]->add(true, 0, 1000);


	stand_up_action = new ServoAction*[NUM_SERVOS];
	for (int i = 0; i < NUM_SERVOS; i++) {
		stand_up_action[i] = new ServoAction(servos[i]);
	}

	// Step 1: Flex back legs
	stand_up_action[SERVO_UFLL]->add(true, 350, 1000);
	stand_up_action[SERVO_UFRL]->add(true, 350, 1000);
	stand_up_action[SERVO_LFLL]->add(true, 0, 1000);
	stand_up_action[SERVO_LFRL]->add(true, 0, 1000);
	stand_up_action[SERVO_UBLL]->add(true, -350, 1000);
	stand_up_action[SERVO_UBRL]->add(true, -350, 1000);
	stand_up_action[SERVO_LBLL]->add(true, -350, 1000);
	stand_up_action[SERVO_LBRL]->add(true, -350, 1000);
	stand_up_action[SERVO_LN]->add(true, 0, 1000);
	stand_up_action[SERVO_UN]->add(true, 0, 1000);
	stand_up_action[SERVO_T]->add(true, 0, 1000);

	// Step 2: Stand up front legs
	stand_up_action[SERVO_UFLL]->add(true, 0, 1000);
	stand_up_action[SERVO_UFRL]->add(true, 0, 1000);
	stand_up_action[SERVO_LFLL]->add(true, 0, 1000);
	stand_up_action[SERVO_LFRL]->add(true, 0, 1000);
	stand_up_action[SERVO_UBLL]->add(true, -350, 1000);
	stand_up_action[SERVO_UBRL]->add(true, -350, 1000);
	stand_up_action[SERVO_LBLL]->add(true, -350, 1000);
	stand_up_action[SERVO_LBRL]->add(true, -350, 1000);
	stand_up_action[SERVO_LN]->add(true, 0, 1000);
	stand_up_action[SERVO_UN]->add(true, 0, 1000);
	stand_up_action[SERVO_T]->add(true, 0, 1000);

	// Step 3: move upper back legs forward
	stand_up_action[SERVO_UFLL]->add(true, 0, 1000);
	stand_up_action[SERVO_UFRL]->add(true, 0, 1000);
	stand_up_action[SERVO_LFLL]->add(true, 0, 1000);
	stand_up_action[SERVO_LFRL]->add(true, 0, 1000);
	stand_up_action[SERVO_UBLL]->add(true, 0, 1000);
	stand_up_action[SERVO_UBRL]->add(true, 0, 1000);
	stand_up_action[SERVO_LBLL]->add(true, -350, 1000);
	stand_up_action[SERVO_LBRL]->add(true, -350, 1000);
	stand_up_action[SERVO_LN]->add(true, 0, 1000);
	stand_up_action[SERVO_UN]->add(true, 0, 1000);
	stand_up_action[SERVO_T]->add(true, 0, 1000);



	// Step 4: All legs straight
	stand_up_action[SERVO_UFLL]->add(true, 0, 1000);
	stand_up_action[SERVO_UFRL]->add(true, 0, 1000);
	stand_up_action[SERVO_LFLL]->add(true, 0, 1000);
	stand_up_action[SERVO_LFRL]->add(true, 0, 1000);
	stand_up_action[SERVO_UBLL]->add(true, 0, 1000);
	stand_up_action[SERVO_UBRL]->add(true, 0, 1000);
	stand_up_action[SERVO_LBLL]->add(true, 0, 1000);
	stand_up_action[SERVO_LBRL]->add(true, 0, 1000);
	stand_up_action[SERVO_LN]->add(true, 0, 1000);
	stand_up_action[SERVO_UN]->add(true, 0, 1000);
	stand_up_action[SERVO_T]->add(true, 0, 1000);
}


/*!
 *
 */
void Robot::update() {
	brain->update();

	for (int i=0;i<NUM_SENSORS;i++) {
		sensors[i]->update();
	}

	for (int i = 0; i < NUM_SERVOS; i++) {
		action[i]->update();
		servos[i]->update();
	}

}

void Robot::initSensors() {
	sensors = new Sensor*[NUM_SENSORS];
	sensors[SENSOR_BUTTON] = button = new Sensor1bit(&PING, 5);
	for (int i = 1; i<NUM_SENSORS; i++) {
		sensors[i] = new Sensor10bit(i-1);
	}
}

void Robot::setAction(ServoAction **action, bool cyclic, bool forward) {
	if (this->action != action) {
		resetAction(action,cyclic,forward);
	}
}
void Robot::resetAction(ServoAction **action, bool cyclic, bool forward) {
	this->action = action;
	for (int i = 0; i < NUM_SERVOS; i++) {
		this->action[i]->reset(cyclic, forward);
	}

}

void Robot::walk_pos0(ServoAction** a, int upperLeg, int lowerLeg, uint32_t duration) {
	a[upperLeg]->add(true, 0, duration);
	a[lowerLeg]->add(true, 0, duration);
}

void Robot::walk_pos1(ServoAction** a, int upperLeg, int lowerLeg, uint32_t duration) {
	a[upperLeg]->add(true, 0, duration);
	a[lowerLeg]->add(true, -300, duration);
}

void Robot::walk_pos2(ServoAction** a, int upperLeg, int lowerLeg, uint32_t duration) {
	a[upperLeg]->add(true, 175, duration);
	a[lowerLeg]->add(true, -350, duration);
}

void Robot::walk_pos3(ServoAction** a, int upperLeg, int lowerLeg, uint32_t duration) {
	a[upperLeg]->add(true, 175, duration);
	a[lowerLeg]->add(true, -175, duration);
}

